#include "stm32f10x.h"                  // Device header
#include "MPU6050_Reg.h"
#include "ENCODER.h"
#include "MPU6050.h"
#include "MOTOR.h"
#include "serial.h"
/*
#define Verical_Kp 0					//正
#define Verical_Kd -80          //负
*/
float Verical_Kp = 380,
		Verical_Kd = -21;

#define Velocity_Kp 0
#define Velocity_Ki 0
#define Low_filter 0.7

#define Turn_Kp -1

#define Med_Value 170
#define Speed_target 0

//PB13 14 控制左电机  PB14 15控制右电机
int16_t Motor1,Motor2;
int16_t A_X,G_Y,G_Z,Speed_Right,Speed_Left;
float Speed_Err,Speed_Err_Last,Speed_Lowout,Speed_Err_S;

/**
   *@brief     直立环
   *@parem  
   *@retval   
   */
int16_t Verical(float Med,float Angle,float gyro_y)
{
	return Verical_Kp*(Angle - Med) + Verical_Kd*(gyro_y - 0);
}

/**
   *@brief     速度环
   *@parem  
   *@retval   
   */
int16_t Velocity(int16_t target,int16_t Speed_Right,int16_t Speed_Left)
{
	Speed_Err = (Speed_Right + Speed_Left) - target;
	//低通滤波
	Speed_Lowout = (1 - Low_filter)*Speed_Err + Low_filter*Speed_Err_Last;
	Speed_Err_Last = Speed_Lowout;
	//积分得路程
	Speed_Err_S += Speed_Lowout;
	//路程限幅
	Speed_Err_S = Speed_Err_S > 10000 ? 10000:Speed_Err_S;
	Speed_Err_S = Speed_Err_S < -10000 ? -10000:Speed_Err_S;
	
	return Velocity_Kp*Speed_Lowout + Velocity_Ki*Speed_Err_S;
}


/**
   *@brief     转向环
   *@parem  
   *@retval   
   */
int16_t Turn(int16_t G_Z)
{
	return Turn_Kp*G_Z;
}

void TIM3_IRQHandler()
{
	if(TIM_GetITStatus(TIM3, TIM_IT_Update) == SET)
	{
		Speed_Right = Encoder_GetTIM2();
		Speed_Left  = Encoder_GetTIM4();
		
		/*
		Serial_SendNumber(Speed_Right);
		Serial_SendString("\r\n");
		Serial_SendNumber(Speed_Left);
		Serial_SendString("\r\n");*/
		
		A_X = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H) << 8 | MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
		G_Y = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H) << 8 | MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
		G_Z = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H) << 8 | MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
		
		int16_t Velocity_out,Vertical_out,Turn_out,PWM_out;
		
		Velocity_out = Velocity(Speed_target,Speed_Right,Speed_Left);
		Velocity_out = 0;
		Vertical_out = Verical(Med_Value/50.0 + Velocity_out,A_X/50.0,G_Y/50.0);
		Turn_out     = Turn(G_Z);
		
		PWM_out = Vertical_out;
		/**/
		Motor1  = PWM_out + Turn_out;
		Motor2  = PWM_out - Turn_out;
		
		Limid(&Motor1,&Motor2);
		Load_SpeedsAndDirections(Motor1,Motor2);
		//Load_SpeedsAndDirections(0,00);
		
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
	}
	
}